////////////////////////////////////
// version 0.4 de Roby
////////////////////////////////////
#include <Servo.h>

// globales
unsigned long _rapport_turrel_last = 0;
unsigned long _rapport_body_last = 0;
unsigned long _rapport_sensors_last = 0;
unsigned long _setting_rapport_turrel_interval = 125*2;
unsigned long _setting_rapport_body_interval = 125*2;
unsigned long _setting_rapport_sensors_interval = 250*2;
boolean _send_status = false;
boolean _send_invalid_cmd = false;

// motors
const int E1 = 5; //M1 Speed Control
const int E2 = 6; //M2 Speed Control
const int M1 = 4; //M1 Direction Control
const int M2 = 7; //M2 Direction Control

// buttons
const int key_S6 = 2, key_S7 = 3, key_S1_5 = 7;

// servos of turrel
Servo servo_turrel_low, servo_turrel_high;

// IF Range Switch
const int infra_front_left = 10;
const int infra_front_right = 9;
const int infra_back_left = 11;
const int infra_back_right = 8;

// IF Range sensors
const int infra_range_turrel = 1;

///////////////
// moteur
void motor_stop(void) //Stop
{
  digitalWrite(E1,LOW);
  digitalWrite(E2,LOW);
}
void motor_action(char a,char b, boolean a2, boolean b2) //Move forward
{
  analogWrite (E1,a); //PWM Speed Control
  digitalWrite(M1,a2?HIGH:LOW);
  analogWrite (E2,b);
  digitalWrite(M2,b2?HIGH:LOW);
}
void setup_motor(void)
{
  pinMode(E1, OUTPUT);
  pinMode(E2, OUTPUT);
  pinMode(M1, OUTPUT);
  pinMode(M2, OUTPUT);
}

//////////////////
// boutons
int check_buttons()
{
  // Cette fonction vérifie les boutons et retourne
  // 0 si pas de bouton appuyé ou sinon 1 à 7 suivant S1...S7
  // Priorités : S7, S6, S1, S2, S3, S4, S5
  if (digitalRead(key_S7)==0) return 7;
  if (digitalRead(key_S6)==0) return 6;
  int w = analogRead(key_S1_5);
#define vS1 0
#define vS2 130
#define vS3 306
#define vS4 478
#define vS5 720
  if ( w < vS2/2 ) return 1;
  if ( w < (vS3+vS2)/2 ) return 2;
  if ( w < (vS4+vS3)/2 ) return 3;
  if ( w < (vS5+vS4)/2 ) return 4;
  if ( w < (1024+vS5)/2 ) return 5;
  return 0;
}
void setup_buttons(){
  pinMode(key_S6, INPUT);
  pinMode(key_S7, INPUT);
}

//////////////
// turrel
const int turrel_low_min = 10, turrel_low_mid = 80, turrel_low_max = 170;
const int turrel_high_min = 30, turrel_high_mid = 85, turrel_high_max = 105;
int current_turrel_position_low = turrel_low_mid;
int current_turrel_position_high = turrel_high_mid;

void reset_turrel_position(int dlay=15)
{
  current_turrel_position_low = turrel_low_mid;
  current_turrel_position_high = turrel_high_mid;
  servo_turrel_low.write(turrel_low_mid); 
  servo_turrel_high.write(turrel_high_mid);
  delay(dlay);    
}
void set_turrel_position(int low, int high)
{
  int set_low = (low<turrel_low_min)?turrel_low_min:(low>turrel_low_max)?turrel_low_max:low;
  int set_high = (high<turrel_high_min)?turrel_high_min:(high>turrel_high_max)?turrel_high_max:high;
  //  int cur_low = servo_turrel_low.read();
  //  int cur_high = servo_turrel_high.read();
  int dlay=15;
  servo_turrel_low.write(set_low); 
  servo_turrel_high.write(set_high);
  current_turrel_position_low = set_low;
  current_turrel_position_high = set_high;
  delay(dlay);
}
void acc_turrel_position(int low, int high)
{
  set_turrel_position(current_turrel_position_low + low, current_turrel_position_high + high);
}

void setup_turrel()
{
  servo_turrel_low.attach(12);
  servo_turrel_high.attach(13);
  reset_turrel_position(50);
}

//////////////
// IF Range Swiches
int setup_rangeswiches(){
  pinMode(infra_front_left, INPUT);
  pinMode(infra_front_right, INPUT);
  pinMode(infra_back_left, INPUT);
  pinMode(infra_back_right, INPUT);
}

//////////////
// IR Ranger Sensors
float get_range_turrel_sensor(int count=10, int dlay=5)
{
  float fDistance = 0;

  for(int i=0; i<count; i++)
  {
    float fScale = 5.0f/1024.0f;    // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
    int iAnalogValue = analogRead(infra_range_turrel);
    float fVolts = iAnalogValue*fScale;

    // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
    fDistance += 65.0f*pow(fVolts, -1.10f); 

    if(dlay>0) delay(dlay);
  }

  return fDistance/count;
}

//////////////
// main
void setup()
{
  Serial.begin(9600); //Set Baud Rate
  Serial.flush();
  setup_motor();
  setup_buttons();
  setup_rangeswiches();
  setup_turrel();
}

//////////////////////////////
// trame
void send_start_trame()
{
  Serial.print("#");
}
void send_end_trame()
{
  Serial.println("$");
}

//////////////////////////////
// version
void send_version()
{
  send_start_trame();
  Serial.write("version:name=roby,version=0.4");
  send_end_trame();
}

//////////////////////////////
// setting
void send_setting()
{
  send_start_trame();
  Serial.print("setting:");
  Serial.print("send_status=");
  Serial.print(_send_status?"1":"0");
  send_end_trame();  
}

void send_setting_body()
{
  send_start_trame();
  Serial.print("setting_body:");
  Serial.print("min_speed=0");
  Serial.print(",max_speed=100");
  Serial.print(",nice_speed=25");
  send_end_trame();
}

void send_setting_turrel()
{
  send_start_trame();  
  Serial.print("setting_turrel:");
  Serial.print("min_low=");
  Serial.print(turrel_low_min);
  Serial.print(",mid_low=");
  Serial.print(turrel_low_mid);
  Serial.print(",max_low=");
  Serial.print(turrel_low_max);
  Serial.print(",min_high=");
  Serial.print(turrel_high_min);
  Serial.print(",mid_high=");
  Serial.print(turrel_high_mid);
  Serial.print(",max_high=");
  Serial.print(turrel_high_max);
  send_end_trame();
}

//////////////////////////////
// status
void send_status_sensors()
{
  boolean fl = (digitalRead(infra_front_left)==HIGH)?true:false;
  boolean fr = (digitalRead(infra_front_right)==HIGH)?true:false;
  boolean bl = (digitalRead(infra_back_left)==HIGH)?true:false;
  boolean br = (digitalRead(infra_back_right)==HIGH)?true:false;
  float dt = get_range_turrel_sensor();

  send_start_trame();
  Serial.print("sensors:");
  Serial.print("fl=");
  Serial.print(fl?"0":"1"); 
  Serial.print(",");
  Serial.print("fr="); 
  Serial.print(fr?"0":"1"); 
  Serial.print(",");
  Serial.print("bl="); 
  Serial.print(bl?"0":"1"); 
  Serial.print(",");
  Serial.print("br="); 
  Serial.print(br?"0":"1"); 
  send_end_trame();
}
void send_status_turrel()
{
  float dist = get_range_turrel_sensor();

  send_start_trame();
  Serial.print("turrel:");
  Serial.print("low=");
  Serial.print(current_turrel_position_low); 
  Serial.print(",");
  Serial.print("high="); 
  Serial.print(current_turrel_position_high); 
  Serial.print(",");
  Serial.print("dist="); 
  Serial.print(dist);
  send_end_trame();
}
void send_status_body()
{
  // future use : need to count time running !
  send_start_trame();
  Serial.print("body:");
  Serial.print("posx=");
  Serial.print(0); 
  Serial.print(",");
  Serial.print("posy="); 
  Serial.print(0); 
  Serial.print(",");
  Serial.print("posz="); 
  Serial.print(0);
  send_end_trame();
}

//////////////////////////////
// serial buffer loading
const int _iMaxSerialBuffer = 256;
int _iSerialBufferStart=0, _iSerialBufferLen=0, _iSerialBufferIndex=0;
char _SerialBuffer[_iMaxSerialBuffer];
boolean readSerialString (String &cmd) {
  int sb;
  while (Serial.available())
  {
    sb = Serial.read();
    if(sb=='#') // start cmd ?
    {
      _iSerialBufferStart = _iSerialBufferIndex+1;
    }
    else if(sb=='$' && _iSerialBufferStart>=0) // end cmd ?
    {
      _iSerialBufferLen = _iSerialBufferIndex;
      _SerialBuffer[_iSerialBufferLen] = '\0';
      cmd = &_SerialBuffer[_iSerialBufferStart];
      _iSerialBufferIndex = 0;
      _iSerialBufferStart = -1;
      return true;
    }
    if(_iSerialBufferIndex>=_iMaxSerialBuffer)
    {
      _iSerialBufferIndex = 0;
      _iSerialBufferStart = -1;
      return false;
    }
    _SerialBuffer[_iSerialBufferIndex] = sb;
    _iSerialBufferIndex++;
  }

  return false;
}

///////////////////////////////
// check commands sended
int load_int(String &s, int start, int &iRet)
{
  int iLen = start+1;
  String num="";

  if (s[start] == '-') num += "-";
  else if(s[start] == '+') num += "";
  else return start;

  for(int i=1; i<6 && s.length()>(start+i); i++)
  {
    char c = s[start+i];
    if(!isDigit(c))
      break;
    num += c;
    iLen ++;
  }

  iRet = num.toInt();
  return iLen;
}

void check_commands()
{
  String line="";
  if(!readSerialString(line))
    return;
  if(line.length()>0)
  {
    // debug only : Serial.println("cmd : "+line);

    switch(line[0])
    {
    case 'v': // (V)ersion
      {
        send_version();
      }
      break;      
    case 'g': // settin(G)
      {
        switch(line[1])
        {
        case 'a': // (A)sk
          {
            switch(line[2])
            {
            case 'b':  // (B)ody setting
              send_setting_body();        
              break;
            case 't':  // (T)urrel setting
              send_setting_turrel();
              break;
            default: // general setting
              send_setting();
              break;
            }
          }
          break;
        }
      } 
      break;            
    case 's': // (S)tatus    
      {
        switch(line[1])
        {
        case 'a': // (A)sk
          {
            switch(line[2])
            {
            case 's':  // (S)ensors
              send_status_sensors();        
              break;
            case 't':  // (T)urrel
              send_status_turrel();
              break;
            case 'b':  // (B)ody
              send_status_body();
              break;
            }
          }
          break;
        case 's': // (S)et
          {
            switch(line[2])
            {
            case 's': // (S)end status automatic
              {
                char param = line[3];
                if(param=='0') _send_status = false;
                else if(param=='1') _send_status = true;
              }
              break;
            }
          }
          break;
        }
      }
      break;
    case 'r': // (R)eset
      {
        switch(line[1])
        {
        case 'b': // (B)ody
          motor_stop();
          break;
        case 't': // (T) turrel
          reset_turrel_position();
          if(!_send_status) send_status_turrel();
          break;
        default: // All !
          motor_stop(); 
          reset_turrel_position();
          if(!_send_status) send_status_turrel();
          break;
        }
      }
      break;      
    case 'b': // (B)ody stuff
      {
        switch(line[1])
        {
        case 'm': // 'M'oving
          {
            boolean bL = (line[2]=='0')?false:true;
            boolean bR = (line[3]=='0')?false:true;
            int iL=0, iR=0;
            int endl = load_int(line, 4, iL);
            load_int(line, endl, iR);
            int iPwnL = map(iL, 0, 100, 50, 250);
            int iPwnR = map(iR, 0, 100, 50, 250);
            motor_action(iPwnL, iPwnR, bL, bR);
          }
        }
      }
      break;      
    case 't': // (T)urrel stuff
      {
        switch(line[1])
        {
        case 'p': // set 'P'osition
          {
            int iLow=0, iHigh=0;
            int endl = load_int(line, 2, iLow);
            load_int(line, endl, iHigh);
            set_turrel_position(iLow, iHigh);
          }
          break;
        }
      }
      break;      
    default:
      if(_send_invalid_cmd)
      {
        send_start_trame();
        Serial.print("error:invalid cmd '");
        Serial.print(line+"'");
        send_end_trame();
      }
      break;
    }
  }
}

///////////////////////////////
// loop
void loop(void)
{
  char val = check_buttons();
  if(val == 0) // no button ?
    check_commands();
  else
  {
    switch(val)
    {
    case 1: //Turn Left
      motor_action(150, 150, true, false);
      break;
    case 2: //Move back
      motor_action(150, 150, false, false);
      break;
    case 3://Turn Right
      motor_action(150, 150, false, true);
      break;
    case 4://Move front
      motor_action(150, 150, true, true);
      break;
    case 5:
      motor_stop();
      break;
    case 6: // reset turrel position
      motor_stop();
      reset_turrel_position();
      break;
    case 7: // enable/disable rapports sending
      _send_status = !_send_status;
      send_setting();
      break;
    }
  }

  if(_send_status)
  {
    unsigned long _rapport_last = millis();
    if(( _rapport_last - _rapport_turrel_last) > _setting_rapport_turrel_interval)
    {
      send_status_turrel();
      _rapport_turrel_last =  _rapport_last;
    }
    if(( _rapport_last - _rapport_body_last) > _setting_rapport_body_interval)
    {
      send_status_body();
      _rapport_body_last =  _rapport_last;
    }
    if(( _rapport_last - _rapport_sensors_last) > _setting_rapport_sensors_interval)
    {
      send_status_sensors();
      _rapport_sensors_last =  _rapport_last;
    }
  }
}




















